#ifndef __PID_H__
#define __PID_H__

#include "system.h"
#include "user_bsp.h"

#define SAMPLE_CACHE_MAX  3

typedef enum
{
    Rol = 0,
    Pit,
    Alt,
    Yaw,
    
    Dof_Num,
}Dof_t;



typedef struct
{
    float Out_Float;
    int16 Out_Int16;

 //   float Out_Min;
 //   float Out_Max;

//    float K_PosErr_To_SpdSet;
    float p;
    float i;
    float d;
    float dd;
	
 //   YesNo_t valid;
 //   YesNo_t valid_last;
    
    float sample;//ֵ
    float sample_last;//
    float sample_cache[SAMPLE_CACHE_MAX];
    uint16 sample_cache_cnt;
//    uint16 sample_cache_max;
    float set;
//   float set_min;
//    float set_max;
    float err;
//    float err_abs;
    float err_last;
//    float err_p;
    float err_i;
//    float err_i_abs_max;
    float err_d;
//    float i_act_err_abs_max;
/*	float err_Last_d;
	float err_spd;
	float err_Last_spd;*/
    
    float out;
    float out_p;
    float out_i;

//    float out_i_min;
//    float out_i_max;
    float out_d;
	float out_spd;
    
    
//    PID_t PID[DofPID_Num];
}Dof_PID_t;

/*typedef struct
{
    float sample;
    float sample_last;
    YesNo_t valid;
    YesNo_t valid_last;
}PID_Sample_t;

typedef struct
{
    PID_Sample_t PosSmp;
    PID_Sample_t SpdSmp;
}Dof_PID_Sample_t;*/

// extern Dof_PID_Sample_t Dof_Alt_Smp;
 extern Dof_PID_t SysPID[Dof_Num];

void PID_Parameter_Init(void);
void PID_Update_Alt_Sample(float NewAltPos_Raw);
uint16 Get_Height(void);
int16 Dof_PID_Update_Calculate(Dof_PID_t* pDof_PID);
YesNo_t Is_Alt_Valid(uint16 Alt_cm);
void PID_Update_Sample(Dof_PID_t* pPID, float Sample_NewRaw, YesNo_t Sample_Valid);
void PID_Update_Set(Dof_PID_t* pPID, float Set_New);
void PID_Update_Error(Dof_PID_t* pPID, YesNo_t Sample_Valid);
float PID_Update_Out(Dof_PID_t* pPID);

//float PID_Update(Dof_PID_t* pPID, float Set, float Smp, YesNo_t SmpValid);

#endif  // __PID_H__

